#include <iostream>
#include <queue>
#include <Utils.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <opencv2/opencv.hpp>
#include "plane_info.h"
using namespace std;
using namespace lyxutils::abbreviations;

//i fucking hate global variable
//but it seems convenient here =>to avoid too lengthy function signature
float s_scale=0.5;
float a_scale=15;
float c_scale=0.5;
float ratio_limit=0.1;//consider good match if match score > ratio_limit*max_score
//consider direction of plane normal when calculating relation feature angle distance
bool consider_direction_in_RF=false;

void printUsage(){
    cout<<"usage:plane_descriptor planes.txt [planes2.txt]"<<endl;
    cout<<"      calculate Objects Vicinity Distribution Feature(OVDF) from file(s)"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h         print this help message"<<endl;
    cout<<"  -k=k1[,k2] use k1 neighbors (k2 neighbors for planes2.txt) to calculate view point"<<endl;
    cout<<"             in order to eliminate normal vector direction uncertainty,defaults are 3"<<endl;
    cout<<"  -n=n1[,n2] use n1 neighbors to describe a plane(default is 50),n2 for planes2.txt"<<endl;
    cout<<"  -o         enable output adjusted plane information to file planes_adjusted.txt"<<endl;
    cout<<"  -m         enable output descriptor match result to matches.txt,this's only effective"<<endl;
    cout<<"             when the second file planes2.txt is given.first two columns in the output"<<endl;
    cout<<"             represents indices of matched planes from planes.txt and planes2.txt respectively"<<endl;
    cout<<"             the following index pairs represents the planes that support the decision of the match"<<endl;
    cout<<"             this option will also enable output of matched plane center points to files align_pts.txt"<<endl;
    cout<<"             and ref_pts.txt respectively(for further transformation matrix calculation)"<<endl;
    cout<<"  -r=ratio   set ratio between lowest match score vs highest one to eliminate bad matches(default 0.1)"<<endl;
    cout<<"  -i         consider direction of plane normal when calculating relation feature distance"<<endl;
    cout<<"  -d         debug mode:output similarity matrix(in image format) and top match scores when -m enabled"<<endl;
    cout<<"             output file name will be similarity.png and top_match_scores.txt respectively"<<endl;
    cout<<"  -s=s_scale set spacial distance scale as s_scale(default:0.5)"<<endl;
    cout<<"  -a=a_scale set normal vector angle distance scale as a_scale(default:15)"<<endl;
    cout<<"  -c=c_scale set object size distance scale as c_scale(default is:0.5)"<<endl;
    cout<<"             note:adjusted_dis=exp(dis/s_scale*3),similarity=1/adjusted_dis, in a word,"<<endl;
    cout<<"             similarity=exp[-(spacial_dis/s_scale+angle_dis/a_scale+size_dis/c_scale)*3]"<<endl;
}

//描述两个平面空间关系的向量
//最后保存时每个平面依次保存其最近到最远平面的关系向量
struct RelationFeature{
    int centerIdx;//base plane index
    int refIdx;//reference plane index
    Vec3f pos;
    Vec3f normal;
    Vec3f scale;
    friend ostream& operator <<(ostream &out,const RelationFeature &rf);
    static Vec3f distance(const RelationFeature &rf1, const RelationFeature &rf2){
        Vec3f dis;
        dis.x=sqrt((rf1.pos-rf2.pos).dot(rf1.pos-rf2.pos));
        float cos_val=rf1.normal.dot(rf2.normal)/rf1.normal.mold()/rf2.normal.mold();
        if(consider_direction_in_RF)cos_val=abs(cos_val);
        dis.y=acos(cos_val)*180/M_PI;
        dis.z=sqrt((rf1.scale-rf2.scale).dot(rf1.scale-rf2.scale));
        return dis;
    }
};

ostream& operator <<(ostream &out,const RelationFeature &rf){
    out<<rf.refIdx<<","<<rf.pos<<","<<rf.normal<<","<<rf.scale;
    return out;
}

Vec3f getCenter(const PlaneInfo &plane){
    vector<Vec3f> vertices;
    plane.getVertices(vertices);
    return np::mean(vertices);
}

Vec3f getScale(const PlaneInfo &plane){
    Vec3f scale=plane.sigma;
    for(int i=0;i<3;++i)scale[i]=sqrt(scale[i]/(plane.ptCount-1));
    //scale represent standard deviation in each direction.except scale.z
    scale.z=sqrt(scale.x*scale.y);
    return scale;
}

float planeArea(const PlaneInfo &plane){
    return (plane.maxXYZ.x-plane.minXYZ.x)*(plane.maxXYZ.y-plane.minXYZ.y);
}

//transform point coordinate according to rotational matrix and scale matrix
//assume P in world coordinate system,P' in local coordinate system
//R=[xAxis,yAxis,zAxis] are vectors of local coordinate system axes x,y,z in world system
//P=R*P'=>P'=R^T*P,consider scale factor(normalize factor)=>P'=R^T*P,P'=P'/scale
//if Q is the origin coordinate of local system,P'=R^T*(P-Q)/scale
// =>P'=transform(P-Q,xAXis,yAxis,zAxis,scale)
Vec3f transform(const Vec3f &pos,const Vec3f &xAxis,const Vec3f &yAxis,const Vec3f &zAxis,Vec3f scale={1,1,1}){
    Vec3f result={xAxis.dot(pos),yAxis.dot(pos),zAxis.dot(pos)};
    result/=scale;
    return result;
}
//read information of planes from file
void loadPlaneInfo(string filename,vector<PlaneInfo> &planes){
    ifstream ifs(xos::abspath(filename));
    string line;
    while(getline(ifs,line)){
        if(!line.empty()){
            PlaneInfo plane;
            plane.parseLine(line);
            planes.push_back(plane);
        }
    }
}
//adjust 3 directions of a plane
//z axis point to view point,y axis point upwards, x axis adjust according to right-hand rule
void adjust3Directions(PlaneInfo &plane,Vec3f pt){
    Vec3f view= pt - plane.center;
    //调整z,x轴方向,注意max,min坐标的变化
    if(view.dot(plane.zAxis)<0){
        plane.zAxis=-plane.zAxis;
        float zTemp=plane.maxXYZ.z;
        plane.maxXYZ.z=-plane.minXYZ.z;
        plane.minXYZ.z=-zTemp;
    }
    Vec3f upwards({0,0,1});
    //adjust y upwards if y is more vertical than x,which is most of the cases
    if(abs(plane.yAxis.dot(upwards))>abs(plane.xAxis.dot(upwards))){
        if(plane.yAxis.dot(upwards)<0){
            plane.yAxis=-plane.yAxis;
            float yTemp=plane.maxXYZ.y;
            plane.maxXYZ.y=-plane.minXYZ.y;
            plane.minXYZ.y=-yTemp;
        }
        Vec3f xDirect=plane.yAxis.cross(plane.zAxis);
        if(xDirect.dot(plane.xAxis)<0){
            plane.xAxis=-plane.xAxis;
            float xTemp=plane.maxXYZ.x;
            plane.maxXYZ.x=-plane.minXYZ.x;
            plane.minXYZ.x=-xTemp;
        }
    }
    else{//adjust x upwards if x is more vertical than y
        if(plane.xAxis.dot(upwards)){
            plane.xAxis=-plane.xAxis;
            float xTemp=plane.maxXYZ.x;
            plane.maxXYZ.x=-plane.minXYZ.x;
            plane.minXYZ.x=-xTemp;
        }
        Vec3f yDirect=plane.zAxis.cross(plane.xAxis);
        if(yDirect.dot(plane.yAxis)<0){
            plane.yAxis=-plane.yAxis;
            float yTemp=plane.maxXYZ.y;
            plane.maxXYZ.y=-plane.minXYZ.y;
            plane.minXYZ.y=-yTemp;
        }
    }
}
//calculate plane features of given planes.the given planes
// may be modified due to the reset of view point
void calculateFeatures(vector<PlaneInfo> &planes,vector<vector<RelationFeature> > &planeFeatures,int num,int k){
    typedef pcl::PointXYZ PT;
    typedef pcl::PointCloud<PT> CloudT;
    CloudT::Ptr cloud(new CloudT());
    for(int i=0;i<planes.size();++i){
        Vec3f center=getCenter(planes[i]);
        cloud->points.push_back(PT(center.x,center.y,center.z));
    }
    cloud->height=1;
    cloud->width=cloud->points.size();
    //create tree and search for neighbors to calculate plane descriptors
    pcl::KdTreeFLANN<PT> tree;
    tree.setInputCloud(cloud);
    for(int i=0;i<cloud->points.size();++i){
        PlaneInfo &basePlane=planes[i];
        vector<int> neighbors;
        vector<float> sqrDis;
        tree.nearestKSearch(cloud->points[i],k,neighbors,sqrDis);
        //adjust basePlane directions
        vector<float> weights;
        vector<Vec3f> centers;
        for(int j=1;j<neighbors.size();++j){
            const PlaneInfo &curPlane=planes[neighbors[j]];
            centers.push_back(getCenter(curPlane));
            weights.push_back(planeArea(curPlane));
        }
        float total=np::sum(weights);
        np::apply(weights,[total](float &w){w/=total;});//normalize weights
        Vec3f viewPt={0,0,0};
        for(int j=0;j<centers.size();++j){
            viewPt+=centers[j]*weights[j];
        }
//        basePlane.setViewPoint(viewPt);
        adjust3Directions(basePlane,viewPt);
    }
    //why do we need two times iteration?because the plane descriptor can
    //only be correct when all planes' normal vector direction been adjusted
    for(int i=0;i<cloud->points.size();++i){
        const PlaneInfo &basePlane=planes[i];
        Vec3f baseCenter=getCenter(basePlane);
        Vec3f baseScale=getScale(basePlane);
        vector<int> neighbors;
        vector<float> sqrDis;
        tree.nearestKSearch(cloud->points[i],num,neighbors,sqrDis);
        planeFeatures.push_back(vector<RelationFeature>());
        //calculate descriptors
        for(int j=1;j<num;++j){
            RelationFeature feature;
            const PlaneInfo &refPlane=planes[neighbors[j]];
            feature.centerIdx=i;
            feature.refIdx=neighbors[j];
            Vec3f refCenter=getCenter(refPlane);
            Vec3f refScale=getScale(refPlane);
            feature.pos=transform(refCenter - baseCenter, basePlane.xAxis, basePlane.yAxis, basePlane.zAxis, baseScale);
            feature.normal=transform(refPlane.zAxis,basePlane.xAxis,basePlane.yAxis,basePlane.zAxis);
            feature.scale=refScale/baseScale;
            (planeFeatures.end()-1)->push_back(feature);
        }
    }
}

void savePlanes(string outFile,const vector<PlaneInfo> &planes){
    ofstream ofs(outFile);
    for(int i=0; i < planes.size(); ++i){
        ofs << planes[i];
    }
    ofs.flush();
    ofs.close();
}
//matched pair,indices of the matched pair and their corresponding score
struct Pair{
    int idx1;
    int idx2;
    float score;
    friend bool operator < (const Pair &p1, const Pair &p2);
};

bool operator < (const Pair &p1, const Pair &p2){
    return p1.score<p2.score;
}
//match row index to col index where values[row][col] has the maximum value in it's row
//but if values[row1][col] and values[row2][col] both are the maximum in their respective rows
//compare values[row1][col] and values[row2][col] and maintain the bigger one;
//the other select the second largest element in it's row and so forth
void make_match(const vector<vector<float> > &values,vector<Pair> &matches, bool progress=false){
//    matches=vector<int>(values.size(),-1);//not matched
    vector<int> matched_row(values.size(), 0);
    vector<int> matched_col(values[0].size(),0);
    matches.clear();
    priority_queue<Pair> q;//big tile top by default
    if(progress){
        cout<<"sort all matrix element:total "<<values.size()<<" rows";
        if(values.size()>0)cout<<" x "<<values[0].size()<<" cols";
        cout<<endl;
    }
    for(int i=0;i<values.size();++i){
        for(int j=0;j<values[i].size();++j){
            q.push(Pair({i,j,values[i][j]}));
        }
        if(progress)xio::consoleProgressBar(i+1,values.size());
    }
    //remaining point pairs to be matched=>the smaller value between rows && cols
    int left=matched_row.size()<matched_col.size()?matched_row.size():matched_col.size();
    if(progress)cout<<"match row to col:"<<endl;
    while(left>0&&!q.empty()){
        const Pair &t=q.top();
        q.pop();
        if(!matched_row[t.idx1]&&!matched_col[t.idx2]){
            matches.push_back(t);
            matched_row[t.idx1]=1;
            matched_col[t.idx2]=1;
            left--;
        }
        if(progress)xio::consoleProgressBar(matched_row.size() - left, matched_row.size());
    }
}

//void make_match(const vector<vector<float> > &values,vector<Pair> &matches, bool progress=false){
//    priority_queue<Pair> result;
//    int size1,size2;
//    size1=values.size();
//    size2=values[0].size();
//    bool used[size2];
//    memset(used,false,sizeof(used));
//    for (int i=0;i<size1;i++){
//        float tmp;
//        tmp=0;
//        int pos;
//        pos=0;
//        for (int j=0;j<size2;j++){
//            if ((!used[j])&&(values[i][j]>tmp)){
//                pos=j;
//                tmp=values[i][j];
//            }
//        }
//        used[pos]=true;
////        matches.push_back({i,pos,tmp});
//        result.push(Pair({i,pos,tmp}));
//    }
//    while(!result.empty()){
//        matches.push_back(result.top());
//        result.pop();
//    }
//}

//calculate each object's similarity to another, and it's matched relation feature(1d vector)
float similarity(const vector<RelationFeature> &feature1,const vector<RelationFeature> &feature2,vector<Pair> &matches){
    vector<vector<float> > similarity_vals(feature1.size(),vector<float>(feature2.size(),0));
    for(int i=0;i<feature1.size();++i){
        for(int j=0;j<feature2.size();++j){
            Vec3f dis=RelationFeature::distance(feature1[i],feature2[j]);
            //feature to feature similarity
            similarity_vals[i][j]=exp((-dis.x/s_scale-dis.y/a_scale-dis.z/c_scale)*3);
        }
    }
    make_match(similarity_vals,matches);
    float sim_score=0;
    for(int i=0;i<matches.size();++i){
        sim_score+=matches[i].score;
    }
    return sim_score;
}
//match planes,result is the matched plane pair indices
//i-th plane in planes1 corresponds to result[i]-th plane in planes2
//result is a m*2 matrix, where each row represents the matched plane indices
//supportPairs is a m*2k matrix,where each row represents neighbor plane index from each point cloud source
// which supports the decision of making i-th plane from src1 and j-th plane from src2 a match
//simMat is the similarity matrix in 8UC1 image format
void matchPlanes(const vector<vector<RelationFeature> > &features1,const vector<vector<RelationFeature> > &features2,
        vector<vector<int> > &result,vector<vector<int> > &supportPairs,cv::Mat &simMat, bool debug)
{
    //calculate similarity matrix of object i from source 1 to object j from source 2
    vector<vector<float> > simVec2D(features1.size(),vector<float>(features2.size(),0));
    //similarity matrix in image format(for convenience of converting to 8 bit image)
    cv::Mat simMatFloat(features1.size(),features2.size(),CV_32FC1);
    //object to object match,each element in row i and col j represents matched feature pairs
    //of object i from point cloud source 1 and object j from point cloud source 2
    vector<vector<vector<Pair> > > matchesCandidates;
    matchesCandidates.resize(features1.size());
    cout<<"calculate similarity between planes from src1(row) and planes from src2(col):"<<endl;
    for(int i=0;i<features1.size();++i){
        matchesCandidates[i].resize(features2.size());
        for(int j=0;j<features2.size();++j){
            float simVal=similarity(features1[i],features2[j],matchesCandidates[i][j]);
            simVec2D[i][j]=simMatFloat.at<float>(i,j)=simVal;
        }
        xio::consoleProgressBar(i+1,features1.size());
    }
    simMatFloat.convertTo(simMat,CV_8UC1);
    //match plane indices
    vector<Pair> simMatches;
    make_match(simVec2D,simMatches,true);
    //select good match
    float max_score=simMatches[0].score;
    result.clear();
    supportPairs.clear();
    cout<<"filter out less perfect match..."<<endl;
    for(int i=0;i<simMatches.size()&& simMatches[i].score > ratio_limit * max_score; ++i){
        result.push_back(vector<int>());
        result[result.size()-1].push_back(simMatches[i].idx1);
        result[result.size()-1].push_back(simMatches[i].idx2);
        const vector<Pair> &rel_match=matchesCandidates[simMatches[i].idx1][simMatches[i].idx2];
        const vector<RelationFeature> &features_plane1=features1[simMatches[i].idx1];
        const vector<RelationFeature> &features_plane2=features2[simMatches[i].idx2];
//        float max_sim=rel_match[0].score;
        //only output first numSupport of supporting relational features
//        int numSupport=3;
        supportPairs.emplace_back();
        for(int j=0;j<rel_match.size();++j){
            supportPairs[supportPairs.size()-1].push_back(features_plane1[rel_match[j].idx1].refIdx);
            supportPairs[supportPairs.size()-1].push_back(features_plane2[rel_match[j].idx2].refIdx);
        }
    }
    if(debug){
        ofstream ofs(xos::pwd()+"/top_match_scores.txt");
        for(int i=0;i<simMatches.size();++i){
            ofs<<simMatches[i].idx1<<","<<simMatches[i].idx2<<","<<simMatches[i].score;
            const vector<Pair> &rel_match=matchesCandidates[simMatches[i].idx1][simMatches[i].idx2];
            for(int j=0;j<rel_match.size();++j){
                ofs<<","<<rel_match[j].score;
            }
            ofs<<endl;
        }
        ofs.flush();
        ofs.close();
    }
}

void saveDescriptors(string filename,const vector<vector<RelationFeature> > &features){
    ofstream ofs(filename);
    cout<<"saving plane descriptor result to "<<filename<<endl;
    for(int i=0;i<features.size();++i){
        if(features[i].size()>0)ofs<<features[i][0];
        for(int j=1;j<features[i].size();++j){
            ofs<<","<<features[i][j];
        }
        ofs<<endl;
    }
    ofs.flush();
    ofs.close();
}

int main(int argc,char **argv) {
    xio::CLParser parser;
    parser.parse(argc,argv);
    vector<string> paras=parser.getParameters();
    //deal with options
    if(paras.size()<1 || parser.hasOption("h")){
        printUsage();
        return 1;
    }
    int k1,k2;
    k1=k2=3;
    int num1,num2;
    num1=num2=50;
    if(parser.hasOption("k")){
        vector<string> ks=str::split(parser.getOptionArg("k"),",");
        if(ks.size()<1){
            cout<<"argument of option -k is wrong,at least one interger should be given"<<endl;
            return 1;
        }
        k2=k1=stoi(ks[0]);
        if(ks.size()>1)k2=stoi(ks[1]);
    }
    if(parser.hasOption("n")){
        vector<string> ns=str::split(parser.getOptionArg("n"),",");
        if(ns.size()<1){
            cout<<"argument of option -n is wrong,at least one interger should be given"<<endl;
            return 1;
        }
        num2=num1=stoi(ns[0]);
        if(ns.size()>1)num2=stoi(ns[1]);
    }
    if(!xos::file(xos::abspath(paras[0]))){
        cout<<"file:"<<paras[0]<<" not exist!"<<endl;
        return 1;
    }
    if(paras.size()==2 && !xos::file(xos::abspath(paras[1]))){
        cout<<"file:"<<paras[1]<<" not exist!"<<endl;
        return 1;
    }
    //load data
    vector<PlaneInfo> planes1;
    vector<PlaneInfo> planes2;
    loadPlaneInfo(paras[0], planes1);
    cout << "total number of planes1:" << planes1.size() << endl;
    if(paras.size()==2){
        loadPlaneInfo(paras[1],planes2);
        cout<<"total number of planes1 in second file:"<<planes2.size()<<endl;
    }
    int numPlanes=paras.size()<2 || planes1.size() < planes2.size() ? planes1.size() : planes2.size();
    if(planes1.size()<10 || (paras.size()==2 && planes2.size()<10)){
        cout<<"total number of planes not enough!(<10)this is not suitable for corresponding plane match"<<endl;
        return 1;
    }
    if(num1>planes1.size()){
        cout<<"number of neighbors("<<num1<<") used to describe the plane(in "
            <<xos::fullname(paras[0])<<") exceed total number of planes,"
            << "reset to " << planes1.size() << endl;
        num1=planes1.size();
    }
    if(paras.size()==2 && num2>planes2.size()){
        cout<<"number of neighbors("<<num2<<") used to describe the plane(in "
            <<xos::fullname(paras[1])<<") exceed total number of planes,"
            << "reset to " << planes2.size() << endl;
        num2=planes2.size();
    }
    //calculate plane features
    vector<vector<RelationFeature> > features1;
    vector<vector<RelationFeature> > features2;
    calculateFeatures(planes1,features1,num1,k1);
    if(paras.size()==2){
        calculateFeatures(planes2,features2,num2,k2);
    }
    //save adjusted planes1(and planes2 if offered)
    if(parser.hasOption("o")){
        string outFile=xos::pwd()+"/"+xos::basename(paras[0])+"_adjusted.txt";
        savePlanes(outFile,planes1);
        if(paras.size()==2){
            string outFile2=xos::pwd()+"/"+xos::basename(paras[1])+"_adjusted.txt";
            savePlanes(outFile2,planes2);
        }
    }
    if(parser.hasOption("m")){
        if(parser.hasOption("r")){
            try{
                ratio_limit=stof(parser.getOptionArg("r"));
            }
            catch(invalid_argument ia){
                cout<<"invalid argument:"<<ia.what()<<endl;
                cout<<"setting -r option failed(arg of -r option must be a float value),use default:"<<ratio_limit<<endl;
            }
        }
        consider_direction_in_RF=parser.hasOption("i");
        //match descriptors
        vector<vector<int> > matchedPlane;
        vector<vector<int> > matchedSupport;
        cv::Mat simMat;
        matchPlanes(features1,features2,matchedPlane,matchedSupport,simMat,parser.hasOption("d"));
        //save result to file
        string outFile=xos::pwd()+"/matches.txt";
        string imgFile=xos::pwd()+"/similarity.png";
        if(parser.hasOption("d"))cv::imwrite(imgFile,simMat);
        ofstream ofs(outFile);
        for(int i=0; i < matchedPlane.size(); ++i){
            ofs << matchedPlane[i][0] << "," << matchedPlane[i][1];
            if(matchedSupport[i].size() > 0){
                ofs << "," << matchedSupport[i][0];
                for(int j=1; j < matchedSupport[i].size(); ++j){
                    ofs << "," << matchedSupport[i][j];
                }
            }
            ofs<<endl;
        }
        ofs.flush();
        ofs.close();
        //output align_pts and ref_pts
        ofstream ofs_align(xos::pwd()+"/"+xos::basename(paras[0])+"_pts.txt");
        ofstream ofs_ref(xos::pwd()+"/"+xos::basename(paras[1])+"_pts.txt");
        //output align_pts.txt and ref_pts.txt,this time output plane lower corners
        // rather than center this is supposed to be more precise than plane center
        ofstream ofs_one(xos::pwd()+"/"+xos::basename(paras[0])+"_corners.txt");
        ofstream ofs_two(xos::pwd()+"/"+xos::basename(paras[1])+"_corners.txt");
        //output plane norms and center for transformation parameter estimation
        ofstream file_alg(xos::pwd()+"/"+xos::basename(paras[0])+"_planes.txt");
        ofstream file_ref(xos::pwd()+"/"+xos::basename(paras[1])+"_planes.txt");
        for(int i=0;i<matchedPlane.size();++i){
            const PlaneInfo &align_plane=planes1[matchedPlane[i][0]];
            const PlaneInfo &ref_plane=planes2[matchedPlane[i][1]];
            Vec3f p1=getCenter(align_plane);
            Vec3f p2=getCenter(ref_plane);
            //output plane centers
            ofs_align<<p1.x<<","<<p1.y<<","<<p1.z<<endl;
            ofs_ref<<p2.x<<","<<p2.y<<","<<p2.z<<endl;
            //output plane centers and norms
            file_alg<<p1<<","<<align_plane.zAxis<<endl;
            file_ref<<p2<<","<<ref_plane.zAxis<<endl;
            //get corners.note:the order matters
            vector<Vec3f> align_nodes,ref_nodes;
            align_plane.getVertices(align_nodes);
            ref_plane.getVertices(ref_nodes);
            auto comp=[](const Vec3f &p1,const Vec3f &p2){
                return p1.z<p2.z;
            };
            sort(align_nodes.begin(),align_nodes.end(),comp);
            sort(ref_nodes.begin(),ref_nodes.end(),comp);
            if((align_nodes[0]-p1).cross(align_nodes[1]-p1).dot(align_plane.zAxis)<0){
                Vec3f tmp=align_nodes[0];
                align_nodes[0]=align_nodes[1];
                align_nodes[1]=tmp;
            }
            if((ref_nodes[0]-p2).cross(ref_nodes[1]-p2).dot(ref_plane.zAxis)<0){
                Vec3f tmp=ref_nodes[0];
                ref_nodes[0]=ref_nodes[1];
                ref_nodes[1]=tmp;
            }
            for(int i=0;i<2;++i){//output first two node=>bottom corners
                ofs_one<<align_nodes[i].x<<","<<align_nodes[i].y<<","<<align_nodes[i].z<<endl;
                ofs_two<<ref_nodes[i].x<<","<<ref_nodes[i].y<<","<<ref_nodes[i].z<<endl;
            }
        }
        //close streams
        ofs_align.close();
        ofs_ref.close();
        //
        ofs_one.close();
        ofs_two.close();
        //
        file_alg.close();
        file_ref.close();
    }
    saveDescriptors(xos::pwd()+"/"+xos::basename(paras[0])+"_ovdf.txt",features1);
    if(paras.size()==2){
        saveDescriptors(xos::pwd()+"/"+xos::basename(paras[1])+"_ovdf.txt",features2);
    }
    return 0;
}